#include "gyro_source.h"
#include "BTreeMap.h"
#include "string.h"


// 释放动态数组内存的函数
void free_dynamic_array(void* array, size_t len, size_t element_size) {
    if (array) {
        char* char_array = (char*)array;
        for (size_t i = 0; i < len; ++i) {
            free(char_array + i * element_size);
        }
        free(array);
    }
}


// 初始化 IMUData 数组的函数
IMUData* init_imu_data(size_t raw_imu_len) {
    IMUData* raw_imu = (IMUData*)malloc(raw_imu_len * sizeof(IMUData));

    if (raw_imu) {
        for (size_t i = 0; i < raw_imu_len; i++) {
            raw_imu[i].timestamp_ms = 0.0;
            for (size_t j = 0; j < 3; j++) {
                raw_imu[i].gyro[j] = 0.0;
                raw_imu[i].accl[j] = 0.0;
                raw_imu[i].magn[j] = 0.0;
            }
        }
    }

    return raw_imu;
}
// 初始化有初值 IMUData
IMUData* init_imu_data(double timestamp_ms, double gyro[3], double accl[3], double magn[3]) {
    IMUData* imu_data = (IMUData*)malloc(sizeof(IMUData));
    if (imu_data) {
        imu_data->timestamp_ms = timestamp_ms;
        for (int i = 0; i < 3; ++i) {
            imu_data->gyro[i] = gyro[i];
            imu_data->accl[i] = accl[i];
            imu_data->magn[i] = magn[i];
        }
    }
    return imu_data;
}

// 释放 IMUData 内存
void free_imu_data(IMUData* imu_data) {
    if (imu_data) {
        free(imu_data);
    }
}


// 初始化 FileMetadata 结构体
FileMetadata* init_file_metadata() {
    FileMetadata* metadata = (FileMetadata*)malloc(sizeof(FileMetadata));
    if (metadata) {
        metadata->imu_orientation = NULL;
        metadata->raw_imu = NULL;
        metadata->raw_imu_len = 10;
        metadata->raw_imu = init_imu_data(metadata->raw_imu_len);
        metadata->quaternions = init_btree_map();
        metadata->gravity_vectors = NULL;
        metadata->image_orientations = init_btree_map();
        metadata->detected_source = NULL;
        metadata->frame_readout_time = 0.0;
        metadata->frame_rate = 0.0;
        metadata->camera_identifier = NULL;
        metadata->lens_profile = NULL;
        metadata->lens_positions = init_btree_map();
        metadata->has_accurate_timestamps = 0;
        metadata->additional_data = NULL;
        metadata->per_frame_time_offsets = NULL;
        metadata->per_frame_data_len = 0;
    }
    return metadata;
}


// 释放 FileMetadata 内存
void free_file_metadata(FileMetadata* metadata) {
    if (metadata) {
        free(metadata->imu_orientation);
        free(metadata->raw_imu);
        free_btree_map(metadata->quaternions);
        free_btree_map(metadata->image_orientations);
        free(metadata->detected_source);
        free(metadata->camera_identifier);
        free(metadata->lens_profile);
        free_btree_map(metadata->lens_positions);
        free(metadata->additional_data);
        free(metadata->per_frame_time_offsets);
        free(metadata);
    }
}

// 初始化 GyroSource
GyroSource* init_gyro_source() {
    GyroSource* gyro_source = (GyroSource*)malloc(sizeof(GyroSource));
    if (gyro_source) {
        gyro_source->duration_ms = 0.0;
        gyro_source->raw_imu = NULL;
        gyro_source->raw_imu_len = 0;
        gyro_source->imu_orientation = NULL;
        memset(gyro_source->imu_rotation_angles, 0, sizeof(double) * 3);
        memset(&gyro_source->imu_rotation, 0, sizeof(Quaternion));
        memset(gyro_source->acc_rotation_angles, 0, sizeof(double) * 3);
        memset(&gyro_source->acc_rotation, 0, sizeof(Quaternion));
        gyro_source->imu_lpf = 0.0;
        memset(gyro_source->gyro_bias, 0, sizeof(double) * 3);
        gyro_source->integration_method = 0;
        gyro_source->quaternions = init_btree_map();
        gyro_source->smoothed_quaternions = init_btree_map();
        gyro_source->org_smoothed_quaternions = init_btree_map();
        gyro_source->use_gravity_vectors = false;
        gyro_source->horizon_lock_integration_method = 0;
        memset(gyro_source->max_angles, 0, sizeof(double) * 3);
        gyro_source->smoothing_status = NULL;
        gyro_source->prevent_recompute = false;
        gyro_source->file_metadata = init_file_metadata();
        gyro_source->file_path = NULL;
        // ... 其他字段的初始化
    }
    return gyro_source;
}

// 释放 GyroSource 内存
void free_gyro_source(GyroSource* gyro_source) {
    if (gyro_source) {
        free(gyro_source->imu_orientation);
        free_imu_data(gyro_source->raw_imu);
        free_btree_map(gyro_source->quaternions);
        free_btree_map(gyro_source->smoothed_quaternions);
        free_btree_map(gyro_source->org_smoothed_quaternions);
        free(gyro_source->smoothing_status);
        free_file_metadata(gyro_source->file_metadata);
        free(gyro_source->file_path);
        // ... 其他字段的释放
        free(gyro_source);
    }
}


int gyro_source_main_test() {
    // 创建 FileMetadata 结构体的示例
    FileMetadata* metadata = init_file_metadata();

    // 赋值字段...

    // 释放 FileMetadata 内存
    free_file_metadata(metadata);

    return 0;
}